#ifndef SERIALCONTROL_H
#define SERIALCONTROL_H

#include "serial/serial.h"
#include <vector>
#include <string>
#include <map>
#include <sstream>
#include <iomanip>
#include <cmath>
#include <chrono>
#include <thread>

/**
 * @class SerialControl
 * @brief 机械臂串口通信控制类
 * 
 * 提供机械臂的串口通信功能，包括角度指令发送和反馈数据接收
 */
class SerialControl {
public:
    /**
     * @brief 串口控制器构造函数
     * @param port 串口设备路径 (如 "/dev/ttyUSB0")
     * @param baudrate 波特率 (如 115200)
     * @param timeout 通信超时时间(ms)，默认1000ms
     */
    SerialControl(const std::string& port, int baudrate, int timeout = 1000);
    
    /**
     * @brief 析构函数，自动关闭串口连接
     */
    ~SerialControl();
    
    /**
     * @brief 发送关节角度指令
     * @param rads 关节角度列表（弧度制）
     * @param machine 控制设备类型，默认为 "arm"
     */
    void sendCommand(const std::vector<double>& rads, const std::string& machine = "arm");

    /**
     * @brief 发送串口指令
     * @param command 规范的串口指令
     */
    void sendCommand(const std::string command);
    
    /**
     * @brief 获取关节角度反馈数据
     * @param machine 控制设备类型，默认为 "arm"
     * @return 关节角度列表（弧度制）
     */
    std::vector<double> getRadsData(const std::string& machine = "arm");
    
    /**
     * @brief 主动关闭串口连接
     */
    void closeSerial();

    /**
     * @brief 设置移动时间
     * @param currentRads 当前关节角度列表（弧度制）
     * @param targetRads 目标关节角度列表（弧度制）
     */

    /**
     * @brief 直接设置移动时间
     * @param moveTime 移动时间(毫秒)
     */
    void setDirectMoveTime(int moveTime);

    /**
     * @brief 释放力
     * @param machine 控制设备类型，默认为 "arm"
     */
    void releaseForce(const std::string& machine = "arm");

    /**
     * @brief 恢复力
     * @param machine 控制设备类型，默认为 "arm"
     */
    void recoverForce(const std::string& machine = "arm");

private:
    /**
     * @brief 生成写入控制命令
     * @param rads 目标关节角度列表
     * @param machine 设备类型
     * @return 格式化后的控制命令字符串
     */
    std::string generateWriteCommand(const std::vector<double>& rads, const std::string& machine);
    
    /**
     * @brief 生成读取反馈命令集
     * @param machine 设备类型
     * @return 读取命令列表
     */
    std::vector<std::string> generateReadCommand(const std::string& machine);
    
    /**
     * @brief 解析PWM反馈数据
     * @param feedbacks 原始反馈字符串列表
     * @return 解析后的关节角度列表（弧度制）
     */
    std::vector<double> parsePWMFeedback(const std::vector<std::string>& feedbacks);
    
    serial::Serial serial_;              ///< 串口通信对象
    std::map<std::string, std::string> idMap_;  ///< 关节ID映射表
    int moveTime_ = 1500; // Default move time in milliseconds
};

#endif // SERIAL_CONTROL_H
